#ifndef BTL_DEMO_CAMERAIMAGEUNDISTORTDEMO_QCAMERAIMAGEUNDISTORTDEMOAPPLICATION
#define BTL_DEMO_CAMERAIMAGEUNDISTORTDEMO_QCAMERAIMAGEUNDISTORTDEMOAPPLICATION

#include <QApplication>
#include <QTimer>
#include <btl/Camera/CameraModelPlumbBob.hpp>
#include <btl/Camera/CameraModelTordoff.hpp>
#include <btl/extra/VideoSource/VideoSourceUEye.hpp>
#include <btl/extra/Gui/QImageView.hpp>
#include <btl/Utility/OpenCVSupport.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <iterator>

class QCameraImageUndistortDemoApplication : public QApplication
{

   Q_OBJECT

   public:

      QCameraImageUndistortDemoApplication(int& argc, char** argv) : QApplication(argc, argv)
      {
         _pVideoSource = new btl::extra::VideoSourceUEye();
         //_pCameraModel = new btl::CameraModelPlumbBob(752,480, 378.92108,378.52942, 374.43253,239.20653, -0.31264,0.09078,-0.00001,-0.00039,0.00000);
         _pCameraModel = new btl::CameraModelTordoff(752,480, 378.92108,378.52942, 374.43253,239.20653, -0.000002177);
         _pTimer = new QTimer;
         _pOriginalView = new btl::extra::QImageViewRGB;
         _pOriginalView->setWindowTitle("Original Image");
         _pOriginalView->show();
         _pUndistortedView = new btl::extra::QImageViewRGB;
         _pUndistortedView->setWindowTitle("Undistorted Image");
         _pUndistortedView->show();
         _frameSize = _pVideoSource->frameSize();
         _mapX = cv::Mat_<float>(_frameSize(1), _frameSize(0));
         _mapY = cv::Mat_<float>(_frameSize(1), _frameSize(0));
         //for each pixel (x,y) in destination (undistorted) image,
         //compute its corresponding coordinates (u,v) in the original (distorted) image
         for(int y = 0; y < _frameSize(1); ++y)
         {
            for(int x = 0; x < _frameSize(0); ++x)
            {
               Eigen::Vector2d undistorted(x, y);
               Eigen::Vector2d distorted = _pCameraModel->distortPoint(undistorted);
               _mapX[y][x] = (float)distorted(0);//
               _mapY[y][x] = (float)distorted(1);//
               //std::cout << "(" << x << "," << y << ") = " << _mapX[y][x] << "\n";
            }
         }
         //std::cout << "_mapX: " << _mapX(0,0) << " " << _mapX(0,1) << " " << _mapX(0,2) << "\n";
         //std::cout << "_mapY: " << _mapY(0,0) << " " << _mapY(0,1) << " " << _mapY(0,2) << "\n";
         //std::cout << "_mapX: " << _mapX(479,749) << " " << _mapX(479,750) << " " << _mapX(479,751) << "\n";
         //std::cout << "_mapY: " << _mapY(479,749) << " " << _mapY(479,750) << " " << _mapY(479,751) << "\n";
         connect(_pTimer, SIGNAL(timeout()), this, SLOT(update()));
         _pTimer->start(30);
      }

      ~QCameraImageUndistortDemoApplication() {}

   protected Q_SLOTS:

      void update()
      {
         btl::ImageRegionConstRGB frame = _pVideoSource->getNextFrame();
         btl::ImageRGB undistorted(_frameSize(0), _frameSize(1));
         cv::Mat cvUndistorted;
         /*
         cv::Mat cameraMatrix = (cv::Mat_<float>(3,3) << 378.92108, 0, 374.43253, 0, 378.52942, 239.20653, 0, 0, 1);
         cv::Mat distCoeffs = (cv::Mat_<float>(5, 1) << -0.31264,0.09078,-0.00001,-0.00039,0.00000);
         cv::Mat R;
         cv::Mat newCameraMatrix;
         cv::Size imageSize(752,480);
         cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, R, newCameraMatrix, imageSize, CV_32FC1, _mapX, _mapY);
         */
         cv::remap(btl::cvUnsafeUnconstImage(frame), cvUndistorted, _mapX, _mapY, cv::INTER_LINEAR, cv::BORDER_CONSTANT);
         int i = 0;
         for(int y = 0; y < _frameSize(1); ++y)
         {
            for(int x = 0; x < _frameSize(0); ++x)
            {
               btl::PixelRGB pixel;
               pixel.red() = cvUndistorted.data[i++];
               pixel.green() = cvUndistorted.data[i++];
               pixel.blue() = cvUndistorted.data[i++];
               undistorted[y][x] = pixel;
            }
         }
         _pOriginalView->setImage(frame);
         _pUndistortedView->setImage(undistorted);
      }

   protected:

      QTimer* _pTimer;

      btl::extra::VideoSource* _pVideoSource;

      Eigen::Vector2i _frameSize;

      btl::CameraModelDistorted* _pCameraModel;

      cv::Mat_<float> _mapX, _mapY;

      btl::extra::QImageViewRGB *_pOriginalView, *_pUndistortedView;

};

#endif //BTL_DEMO_CAMERAIMAGEUNDISTORTDEMO_QCAMERAIMAGEUNDISTORTDEMOAPPLICATION
